/**
* @file main.cpp
* @brief This code is used to capture rgb images and ir (infrayed) image simultaneously using a kinect device.
* - The dependency includes opencv and freenect.
* - freenect can be installed from apt-get, but the libfreenect_sync.h file might require being installed manually.
* - ussage: " press c to capture; q to exist. \n"
* - note that RGB and IR image cannot be displayed simultaneously.
* - Only the IR image is shown, if all the centers of circles are detected the markers will shown, then the user may
*   press c to capture both the ir and rgb image simultaneously.
* - the output images will be stored in the local folder.
* @author Shuda Li<lishuda1980@gmail.com>
* @version 1.0
* @date 2011-06-26
*/

#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <libfreenect_sync.h>
#include <boost/lexical_cast.hpp>
#include <stdio.h>
#include <iostream>
#include <string>
#include <vector>
#include <Converters.hpp>

using namespace std;
using namespace btl; //for "<<" operator
using namespace utility;

int main()
{
    cout << " press c to capture; q to exist. \n";

    cv::Mat cvmRGB ( cvSize ( 640, 480 ), CV_8UC3 );
    cv::Mat cvmIR ( cvSize ( 640, 480 ), CV_8UC1 );
    cv::Mat cvmTmp ( cvSize ( 640, 480 ), CV_8UC3 ); // for rendering the checkboard
    cv::Mat cvmBW ( cvSize ( 640, 480 ), CV_8UC1 );


    if ( true ) // use corner patterns
    {
        std::vector<cv::Point2f> _vCurrentCorners;

        int key = cvWaitKey ( 10 );
        int n = 0;

        cv::namedWindow ( "IR", 1 );
        cv::namedWindow ( "IRGrid", 1 );

    
        while ( true )
        {
            char* data1;
            char* data2;
            unsigned int timestamp;
            freenect_sync_get_video ( ( void** ) ( &data1 ), &timestamp, 0, FREENECT_VIDEO_IR_8BIT );

            int i = 0;
            for ( int y = 0; y < cvmIR.rows; y++ )
                for ( int x = 0; x < cvmIR.cols; x++ )
                {
                    unsigned int uIdx = y * cvmIR.cols + x;
                    cvmIR.data[i++] = data1[ uIdx ];
                }
/*
            freenect_sync_get_video ( ( void** ) ( &data2 ), &timestamp, 0, FREENECT_VIDEO_RGB );
            for ( int y = 0; y < cvmRGB.rows; y++ )
                    for ( int x = 0; x < cvmRGB.cols; x++ )
                    {
                        unsigned int uIdx = y * cvmRGB.cols + x;
                        cvmRGB.data[i++] = data2[ 3 * uIdx ];
                        cvmRGB.data[i++] = data2[ 3 * uIdx + 1 ];
                        cvmRGB.data[i++] = data2[ 3 * uIdx + 2 ];
                    }

            cv::cvtColor ( cvmRGB, cvmBW, CV_RGB2GRAY );
*/
            cv::Size boardSize ( 8, 6 );
            //cv::Size patternSize(4,11);// asymmetric circle grid

            bool _bChessBoardCornersFoundThisFrame =
                cv::findChessboardCorners ( cvmIR, boardSize, _vCurrentCorners, CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FILTER_QUADS );
//            cv::findCirclesGrid( cvmIR, boardSize, _vCurrentCorners, CALIB_CB_CLUSTERING | CALIB_CB_SYMMETRIC_GRID);

            if ( _bChessBoardCornersFoundThisFrame )
            {
                cv::cornerSubPix ( cvmIR, _vCurrentCorners, cv::Size ( 11, 11 ), cv::Size ( -1, -1 ), cv::TermCriteria ( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 30, 0.1 ) );
                cv::cvtColor ( cvmIR, cvmTmp, CV_GRAY2RGB );

                cv::drawChessboardCorners ( cvmTmp, boardSize, _vCurrentCorners, _bChessBoardCornersFoundThisFrame );
            }

            PRINT( _vCurrentCorners.size() );

            key = cvWaitKey ( 10 );
            //cout << key << endl;

            if ( key == 'c' ) //c
            {
                cout << "c pressed... " << endl;
                freenect_sync_get_video ( ( void** ) ( &data2 ), &timestamp, 0, FREENECT_VIDEO_RGB );

                i = 0;

                for ( int y = 0; y < cvmRGB.rows; y++ )
                    for ( int x = 0; x < cvmRGB.cols; x++ )
                    {
                        unsigned int uIdx = y * cvmRGB.cols + x;
                        cvmRGB.data[i++] = data2[ 3 * uIdx ];
                        cvmRGB.data[i++] = data2[ 3 * uIdx + 1 ];
                        cvmRGB.data[i++] = data2[ 3 * uIdx + 2 ];
                    }

                //capture depth map
                cv::cvtColor ( cvmRGB, cvmRGB, CV_BGR2RGB );
                std::string strNum = boost::lexical_cast<string> ( n );
                std::string strRGBFileName = "rgb" + strNum + ".bmp"; //saved into the folder from which the KinectCalibrationDemo is being run.
                std::string strIRFileName  = "ir"  + strNum + ".bmp";
                cv::imwrite ( strRGBFileName.c_str(), cvmRGB );
                cv::imwrite ( strIRFileName.c_str(),  cvmIR );
                n++;
            }

            if ( key == 'q' ) //q 1048689
            {
                break;
            }

            cv::imshow ( "IR",     cvmTmp );
            cv::imshow ( "IRGrid", cvmIR );
        }
    }
   
    freenect_sync_stop();

    return EXIT_SUCCESS;
}
